package upm.etsit.gsi.lejos.mazesolver;

import java.io.*;

import upm.etsit.gsi.lejos.mazesolver.behaviours.Exploration;
import upm.etsit.gsi.lejos.mazesolver.behaviours.GoStraight;
import upm.etsit.gsi.lejos.mazesolver.behaviours.Mapper;
import upm.etsit.gsi.lejos.mazesolver.behaviours.TurnLeft;
import upm.etsit.gsi.lejos.mazesolver.behaviours.TurnRight;
import upm.etsit.gsi.lejos.mazesolver.behaviours.Win;

import lejos.nxt.*;
import lejos.nxt.addon.ColorSensor;
import lejos.robotics.subsumption.*;
import lejos.robotics.navigation.*;

/**
 * Clase principal, encargada de crear e iniciar el árbitro y sus Behaviors
 * correspondientes.
 * 
 * @author jrubia
 */
public class NXT {

	/**
	 * Intercambiador de datos entre todas las clases.
	 */
	private DataExchange DE;

	/**
	 * Piloto encargado del movimiento del robot.
	 */
	private TachoPilot pilot;

	/**
	 * Árbitro encargado de manejar todos los Behaviors.
	 */
	private Arbitrator arbitrator;

	/**
	 * Sensor de ultrasonidos para medir las distancias.
	 */
	private UltrasonicSensor sonar;

	/**
	 * Sensor de color.
	 */
	private ColorSensor color;

	/**
	 * Mapeador
	 */
	private Mapper mapper;

	/**
	 * Constructor con parámetros
	 * 
	 * @param wheelDiameter
	 *            Diámetro de la rueda, en milímetros.
	 * @param trackWidth
	 *            Distancia entre ejes centrales de cada rueda, en milímetros.
	 * @param Izquierdo
	 *            Motor izquierdo.
	 * @param Derecho
	 *            Motor derecho.
	 * @param sonar
	 *            Puerto del sensor de ultrasonidos.
	 * @param color
	 *            Puerto del sensor de color.
	 */
	public NXT(float wheelDiameter, float trackWidth, Motor Izquierdo,
			Motor Derecho, SensorPort sonar, SensorPort color) {

		this.pilot = new TachoPilot(wheelDiameter, trackWidth, Izquierdo,
				Derecho);
		this.mapper = new Mapper();
		this.sonar = new UltrasonicSensor(sonar);
		this.color = new ColorSensor(color);

		this.DE = new DataExchange(this.getSonar());

		Behavior[] behaviors = {
				new GoStraight(this.getPilot(), getDE(), this.mapper),
				new TurnRight(this.pilot, this.DE, this.mapper),
				new TurnLeft(this.pilot, this.DE, this.mapper),
				new Win(this.color, this.mapper),
				new Exploration(this.getPilot(), getDE(), this.mapper) };

		this.arbitrator = new Arbitrator(behaviors);

	}

	/**
	 * @return the DE
	 */
	public DataExchange getDE() {
		return DE;
	}

	/**
	 * @param DE
	 *            the DE to set
	 */
	public void setDE(DataExchange DE) {
		this.DE = DE;
	}

	/**
	 * @return the piloto
	 */
	public TachoPilot getPilot() {
		return pilot;
	}

	/**
	 * @param pilot
	 *            the pilot to set
	 */
	public void setPilot(TachoPilot pilot) {
		this.pilot = pilot;
	}

	/**
	 * @return the arbitrator
	 */
	public Arbitrator getArbitrator() {
		return arbitrator;
	}

	/**
	 * @param arbitro
	 *            the arbitro to set
	 */
	public void setArbitro(Arbitrator arbitro) {
		this.arbitrator = arbitro;
	}

	/**
	 * @return the sonar
	 */
	public UltrasonicSensor getSonar() {
		return sonar;
	}

	/**
	 * @param sonar
	 *            the sonar to set
	 */
	public void setSonar(UltrasonicSensor sonar) {
		this.sonar = sonar;
	}

	/**
	 * @return the color
	 */
	public ColorSensor getColor() {
		return color;
	}

	/**
	 * @param color
	 *            the color to set
	 */
	public void setColor(ColorSensor color) {
		this.color = color;
	}

	/**
	 * Inicia la ejecución del programa. Crea un objeto de la clase NXT e inicia
	 * el árbitro del objeto creado anteriormente
	 * 
	 * @param args
	 */
	public static void main(String[] args) throws IOException,
			IllegalStateException, InterruptedException {
		LCD.drawString("Funciono", 0, 0);
		NXT nxt = new NXT(56, 110, Motor.A, Motor.C, SensorPort.S1,
				SensorPort.S2);

		nxt.getArbitrator().start();
	}
}
